/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "stdbool.h"
#include "OLED.h"
#include "math.h"
#include "stdlib.h"


#define SPEED 65
#define AVR 3


/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

	int buff;
	int buff_t=0;
	int buff_x=0;
	int buff_L=0;
	
	
	int x[AVR]={0};
	int x_count=0;
	int x_bar=0;
	
	int output3=0;
	int output4=0;
	static char flag;
	static short int od=0;
	
	float vp,vi,vd;
	float ap,ai,ad;
	
	int v3;
	int v4;
	
uint8_t order=0;
uint8_t RxByte;

uint8_t RxBuff[256];
uint8_t LRxBuff[256];

uint16_t Rx_Count;
uint16_t LRx_count;

uint32_t wheel_speed[4][2];
uint16_t wheel_diff[4];

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
struct pidstruct vpid3,vpid4,angpid3;


/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
struct pidstruct
{
	float kp,ki,kd;
	float p,i,d;
	float thisd,lastd;
	float target;
};

struct pidstruct initpid(float kp,float ki,float kd,float target,struct pidstruct e)
{
	e.kp=kp;
	e.ki=ki;
	e.kd=kd;
	e.p=0;
	e.i=0;
	e.d=0;
	e.lastd=0;
	e.thisd=0;
	e.target=target;
	return e;
}

float PID(struct pidstruct *e,float actualvalue,float minoutput,float maxoutput)
{
	float out;
	e->thisd=e->target-actualvalue;
	e->p=(e->kp)*(e->thisd);

	e->i=(e->ki)*((e->i)+(e->thisd));

	e->d=(e->kd)*((e->thisd)-(e->lastd));
	e->lastd=e->thisd;
	out=(e->p)+(e->i)+(e->d);
	if(e->i>maxoutput)e->i=0;
		if(e->i<minoutput)e->i=0;
	if(out>maxoutput) out=maxoutput;
	if(out<minoutput) out=minoutput;
	return out;
}
int fputc(int ch, FILE *f)
{
  HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);
  return ch;
}


int buff_count(int k,uint8_t RxBuff[],int tf)
{
	int buff;
	int i;
	switch(k)
			{
				case 0:
					break;
				case 1:
					break;
			//1bit	
				case 2:
				buff=RxBuff[0]-48;
				 buff=tf*buff;
				break;
				//2bit
				case 3:
				buff=10*(RxBuff[0]-48)+(RxBuff[1]-48);
			  buff=tf*buff;
				break;
				//3bit
				case 4:
				buff=100*(RxBuff[0]-48)+10*(RxBuff[1]-48)+(RxBuff[2]-48);
			   buff=tf*buff;
				break;
				//4bit
				case 5:
					buff=1000*(RxBuff[0]-48)+100*(RxBuff[1]-48)+10*(RxBuff[2]-48)+(RxBuff[3]-48);
			     buff=tf*buff;
				break;
				
				default:
					break;
			}
	return buff;
}

void stop(){
		__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3,1000); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,1);
		
	__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4,1000); 
			HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15,1);
		
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
	
}
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void wheel(int num,int pwm)
{
	int direct;
	if(pwm>1000)
	{
		pwm=1000;
	}
	if(pwm<-1000)
	{
		pwm=-1000;
	}

	if(pwm>=0)
	{
		direct=1;
	}
	else direct=0;
	
	switch(num){
		case 1:
			switch(direct){
				case 1:
			__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,0);
			break;
		case 0:
			__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,1000+pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,1);
		break;
	}
	break;
		case 2:
			switch(direct){
				case 1:
			__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,1000-pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_4,1);
			break;
		case 0:
			__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,-pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_4,0);
		break;
	}
	break;
	
	case 3:
			switch(direct){
				case 1:
			__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3,pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,0);
			break;
		case 0:
			__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3,1000+pwm); 
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,1);
		break;
	}
	break;
	
	case 4:
			switch(direct){
				case 1:
			__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4,1000-pwm); 
			HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15,1);
			break;
		case 0:
			__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4,-pwm); 
			HAL_GPIO_WritePin(GPIOA,GPIO_PIN_15,0);
		break;
	}
	break;
	
	}
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
}


void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
	static int cur = 0;

	wheel_speed[2][cur] = HAL_TIM_ReadCapturedValue(&htim1,TIM_CHANNEL_3);
	wheel_speed[3][cur] = HAL_TIM_ReadCapturedValue(&htim1,TIM_CHANNEL_4);
	for (int i = 2; i < 4; ++i) {
		if (wheel_speed[i][cur] == wheel_speed[i][cur ^ 1]) {
			continue;
		}
		wheel_diff[i] = wheel_speed[i][cur] - wheel_speed[i][cur ^ 1];
		int a = 10*523.6/wheel_diff[3];
		int b=10*523.6/wheel_diff[2];
//		printf("%d\r", a);
//		printf("%d\r", b);
//		printf("%d\n",wheel_diff[3]);
//		print_double(523.6/wheel_diff[3]);
	}
	cur ^= 1;
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
		// UNUSED(huart);
	if(huart==&huart3){

	static short int  t;
	if(Rx_Count>=255) //???????,??,????????
	{
		Rx_Count=0;
		uint8_t errInfo[]="too many data!!!\r\n";
		while(HAL_UART_Transmit_IT(&huart3,errInfo,18)==HAL_OK);
	}
	else 
	{
		if (RxByte=='s')
		{//s--stop for 5 s -||-
			od=0;
			flag='s';
	      //printf("#");
		}
		else if(RxByte=='p')//stop untill 'd' -|-
		{
			od=0;
		}
		else if(RxByte=='d')//start
		{
			od=1;
		}
		else if(RxByte==0x24){//$--buff_x
			t=1;
			//printf("$");
		}
		else
 {
			
		RxBuff[Rx_Count++]=RxByte;
			
		if(RxByte==0x0D)
	  {//\r-
			int k;
			int tf;
		
			 k=Rx_Count;
			 tf=-1;
			Rx_Count=0;
		
			buff=buff_count(k,RxBuff,tf);
		
			switch(t)
			{
				case 0:
					//buff_t=buff;
				  break;
				case 1:
					buff_x=buff;
					break;
				default:
					break;
			}
	}
		
		if(RxByte==0x0A)//\n+
	  { int k;
			int tf;
			
			k=Rx_Count;
		  tf=1;
			Rx_Count=0;
			
			buff=buff_count(k,RxBuff,tf);
			
			switch(t)
			{
				case 0:
					//buff_t=buff;
				  break;
				case 1:
					buff_x=buff;
					break;
				default:
					break;
			}
			 //printf("%d",buff);
			//printf("1:%d\r\n",x_bar);
			 //printf("%d\r\n",buff_t);
				}
		
			}
		}	
	while(HAL_UART_Receive_IT(&huart3,&RxByte,1)==HAL_OK);

	}
else if(huart==&huart1){
	static short int  t1;
	static short int  t2;
		if(order=='S')
		{
			od=0;
		}
		else if(order=='D')
		{
			od=1;
		}
		else if(order=='a')
		{
			t1=0;
		}
		else if(order=='v')
		{
			t1=1;
		}
		else if(order=='p')
		{
			t2=0;
		}
		else if(order=='i')
		{
			t2=1;
		}
		else if(order=='d')
		{
			t2=2;
		}
		else
		{
			LRxBuff[LRx_count++]=order;
			
		if(order=='l')//L-end
	  {
			int k;
			
			k=LRx_count;
			LRx_count=0;
		
			buff_L=buff_count(k,LRxBuff,1);
		 
		if(t1==0){//a
			switch(t2){
				case 0:
					ap=0.001*buff_L;
					angpid3.kp=ap;
				printf("ap:%.2f\r\n",ap);
				break;
				case 1:
					ai=0.01*buff_L;
				angpid3.ki=ai;
				printf("ai:%.2f\r\n",ai);
				break;
				case 2:
					ad=0.01*buff_L;
				angpid3.kd=ad;
				printf("ad:%.2f\r\n",ad);
				break;
				default:
						break;
			}

			
		}
		else if(t1==1){//v
			
			switch(t2){
				case 0:
					vp=0.01*buff_L;
				vpid3.kp=vp;
				vpid4.kp=vp;
				printf("vp:%.2f\r\n",vp);
				break;
				case 1:
					vi=0.01*buff_L;
				vpid3.ki=vi;
				vpid4.ki=vi;
				printf("vi:%.2f\r\n",vi);
				break;
				case 2:
					vd=0.01*buff_L;
				vpid3.kd=vd;
				vpid4.kd=vd;
				printf("vd:%.2f\r\n",vd);
				break;
				default:
						break;
			        }
			
		       }
			 //printf("%d",buff);
			//printf("1:%d\r\n",x_bar);
			// printf("%d\r\n",buff_t);
					 
					 
//					vpid3=initpid(vp,vi,vd,SPEED,vpid3);
//					vpid4=initpid(vp,vi,vd,SPEED,vpid4);
//					angpid3=initpid(ap,ai,ad,0,angpid3);		
					 
	       }
		
		
	
			
	    }
		while(HAL_UART_Receive_IT(&huart1,&order,1)==HAL_OK);
											}

}
	



void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	static uint32_t time=0;
	time++;
	int temp=0; 
	static unsigned long int timeforstop;
	
	x_bar=buff_x;
//stop for 5 s
if(flag=='s'){
	timeforstop++;
	if(timeforstop%5000==0){
		od=1;
		flag=0;
		timeforstop=0;
	}
}
////////////////

	if(time%30==0)
	{
		
		
		//temp=x_bar;
		temp=x_bar;
	
		vpid3.target=SPEED+PID(&angpid3,-temp,-SPEED+30,SPEED-30);
		vpid4.target=SPEED-PID(&angpid3,-temp,-SPEED+30,SPEED-30);
		
		v3=52360 / wheel_diff[2];
		v4=52360 / wheel_diff[3];
		
		if(time%100==0)
		{
			
		printf("1:%d\r\n",   x_bar);
//		printf("1:%d\r\n",   x_bar);
//		printf("3:%d\r\n",	(int)vpid3.target);
//		printf("4:%d\r\n",  (int)vpid4.target);
		printf("3:%d\r\n",	v3);
		printf("4:%d\r\n",  v4);
		}

	}
	
	output3=PID(&vpid3, v3,0,+1000);
	output4=PID(&vpid4, v4,0,+1000);
	
	//if(order==0){output3=0;output4=0;}
	if(od==0)
	{
	stop();
//		wheel(3,0);
//		wheel(4,0);
	}
	else{
		wheel(3,output3);
		wheel(4,output4);
	}
		
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM4_Init();
  MX_USART1_UART_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */
	OLED_Init();

	HAL_TIM_IC_Start_IT(&htim1,TIM_CHANNEL_3);
	HAL_TIM_IC_Start_IT(&htim1,TIM_CHANNEL_4);
	HAL_UART_Receive_IT(&huart3,&RxByte,1);
	HAL_UART_Receive_IT(&huart1,&order,1);
	HAL_TIM_Base_Start_IT(&htim4);
	
	
	vpid3=initpid(60,1,0.2,SPEED,vpid3);
	vpid4=initpid(60,1,0.2,SPEED,vpid4);
//	vpid3.kp=60;
//	vpid3.ki=1;
//	vpid3.kd=0.8;
//	vpid3.target=SPEED;
//	
//	vpid4.kp=60;
//	vpid4.ki=1;
//	vpid4.kd=0.8;
//	vpid4.target=SPEED;
	
	//angpid3=initpid(0.09,0,1.5,0,angpid3);
	angpid3.kp=0.11;
	angpid3.kd=2;
	angpid3.ki=0;
	angpid3.target=0;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while(1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
